import time
import image
import sensor
from pyb import UART
from pyb import LED
THRESHOLD = (0, 46, -58, 54, -88, 36)  # 循迹阈值


uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)  # 8位数据位，无校验位，1位停止位

# LED(1).on()
# LED(2).on()
# LED(3).on()

sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
# 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
sensor.set_framesize(sensor.QQQVGA)
# sensor.set_windowing([0,20,80,40])
# WARNING: If you use QQVGA it may take seconds
sensor.skip_frames(time=2000)
clock = time.clock()                # to process a frame sometimes.

while (True):
    clock.tick()
    img = sensor.snapshot()
    img.mean(2)  # 降噪
    img.binary([THRESHOLD])  # 二值化
    img.crop([0, 15, 80, 60])  # 截取图像
    img.erode(1)  # 加粗线条
    line = img.get_regression([(100, 100)], robust=True)
    if (line):
        rho = (int)(abs(line.rho())-img.width()/2)
        if line.theta() > 90:
            theta = line.theta()-180
        else:
            theta = line.theta()
        img.draw_line(line.line(), color=127)

        data = bytearray([0xb3, 0xb3, rho, theta, 0x5b])
        uart.write(data)  # 打印rho和偏移坐标
        print("rho:", rho)  # 回归线的偏移距离   一般用偏移距离较好
        print("theta:", theta)  # 回归线的偏移角度
